%%----------------------------------------------------
%% A*寻路算法
%% @author whjing2011@gmail.com
%%----------------------------------------------------
-module(a_path).
-export([
        test/1
        ,test_vast/1
        ,smooth_line/2
        ,to/1
        ,find/4
        ,find_vast/4
    ]
).
-define(a_path_type_1(X, Y), {X - 1, Y}).
-define(a_path_type_2(X, Y), {X + 1, Y}).
-define(a_path_type_3(X, Y), {X, Y - 1}).
-define(a_path_type_4(X, Y), {X, Y + 1}).
-define(a_path_type_5(X, Y), {X - 1, Y - 1}).
-define(a_path_type_6(X, Y), {X - 1, Y + 1}).
-define(a_path_type_7(X, Y), {X + 1, Y + 1}).
-define(a_path_type_8(X, Y), {X + 1, Y - 1}).
-define(next_p(S, T), if S > T -> S - 1; S < T -> S + 1; true -> S end).
-record(a_path, {
        %% 当前点
        node = {0, 0}           :: {non_neg_integer(), non_neg_integer()}
        %% 父节点
        ,parent_node            :: undefined | {non_neg_integer(), non_neg_integer()}
        %% 与父节点关系类型 0:没有 1:X-1 2:X+1 3:Y-1 4:Y-1 5:X-1,Y-1, 6:X-1,Y+1 7:X+1,Y+1 8:X+1,Y-1
        ,type = 1               :: 1..3
        %% G值 从起点 A 移动到指定方格的移动代价，沿着到达该方格而生成的路径
        ,g = 0                  :: non_neg_integer()
        %% H值 从指定的方格移动到终点 B 的估算成本
        ,h = 0                  :: non_neg_integer()
        %% F值 F = G + H
        ,f = 0                  :: non_neg_integer()
        %% 附加参数
        ,data                   :: any()
        %% 左树
        ,l_tree                 :: undefined | #a_path{}
        %% 左树节点数量
        ,l_num = 0              :: non_neg_integer()
        %% 右树
        ,r_tree                 :: undefined | #a_path{}
        %% 右树节点数量
        ,r_num = 0              :: non_neg_integer()
        %% 状态
        ,status = open          :: open | close
    }
).
-record(a_path_tmp, {
        %% 起点X
        sx = 0          :: non_neg_integer()
        %% 起点Y
        ,sy = 0         :: non_neg_integer()
        %% 终点X
        ,tx = 0         :: non_neg_integer()
        %% 终点Y
        ,ty = 0         :: non_neg_integer()
        %% 左右移动代价
        ,lf_cost = 0    :: non_neg_integer()
        %% 上下移动代价
        ,ud_cost = 0    :: non_neg_integer()
        %% 斜角移动代价
        ,diag_cost = 0  :: non_neg_integer()
        %% 连通点 中转站 跳点
        ,tran = []      :: [#a_path{}]
        %% 如果相邻有不可走区域 是否走斜角
        ,diag = false   :: false | true
    }
).
%% -define(print(F, A), io:format(lists:concat(["[~w:~w]", F]), [?MODULE, ?LINE | A])).
-define(print(F, A), ok).

%% 测试
test(Num) ->
    Walkable = map_data_walkable:get(10006),
    erlang:statistics(wall_clock),
    L = to(find({7, 79}, {8, 23}, Walkable, [{num, Num}, {cost, 30}])),
    {_, _Time} = erlang:statistics(wall_clock),
    ?print("use time: ~w ms~n", [_Time]),
    L.
test_vast(Num) ->
    Walkable = map_data_walkable:get(10006),
    erlang:statistics(wall_clock),
    L = to(find_vast({7, 79}, {8, 23}, Walkable, [{num, Num}])),
    {_, _Time} = erlang:statistics(wall_clock),
    ?print("use time: ~w ms~n", [_Time]),
    L.

%% @doc 查找结果转化
-spec to([#a_path{}]) -> list().
to([L | _]) -> 
    to(L, []);
to([]) ->
    [].
to([], L) ->
    lists:reverse(L);
to([#a_path{node = P, data = undefined} | T], L) ->
    to(T, [P | L]);
to([#a_path{data = Data} | T], L) ->
    to(T, [Data | L]).

%% @doc 平滑线段处理
-spec smooth_line([#a_path{}], [{pos_integer(), pos_integer()}]) -> [#a_path{}].
smooth_line(L, Walkable) ->
    D = dict:from_list([{P, 1} || P <- Walkable]),
    NewL = do_smooth_line(L, []), %% 线平dict理
    do_smooth_triangle(NewL, D, []). %% 三角平滑处理
%% 线平滑处理
do_smooth_line([], L) ->
    L;
do_smooth_line([{X1, Y1}, P2 = {X2, Y2}, P3 = {X3, Y3}, P4 = {X4, Y4} | T], L) when (X1 - X2) * (Y3 - Y4) =:= (X3 - X4) * (Y1 - Y2) ->
    do_smooth_line([P2, P3, P4 | T], L);
do_smooth_line([P1 | T], L) ->
    do_smooth_line(T, [P1 | L]).
%% 三角平滑处理
do_smooth_triangle([], _Walkable, L) ->
    L;
do_smooth_triangle([P], _Walkable, L) ->
    [P | L];
do_smooth_triangle([P1, P2], _Walkable, L) ->
    [P2, P1 | L];
%% do_smooth_triangle([{X1, Y1}, {X2, Y2}, {X3, Y3} | T], Walkable, L) when abs(X1 - X3) + abs(Y1 - Y3) >= 30 ->
%%     do_smooth_triangle([{X2, Y2}, {X3, Y3} | T], Walkable, [{X1, Y1} | L]);
do_smooth_triangle([{X1, Y1}, {X2, Y2}, {X3, Y3} | T], Walkable, L = [{X0, Y0} | _]) when abs(X0 - X1) =:= abs(Y0 - Y1) -> %% 最后为斜线方法，则本次优化斜线
    case triangle_straight({X1, Y1}, {X3, Y3}, Walkable, []) of 
        [] -> %% 直达
            do_smooth_triangle([{X1, Y1}, {X3, Y3} | T], Walkable, L);
        [I = {_, _}] -> %% 有转角
            do_smooth_triangle([I, {X3, Y3} | T], Walkable, [{X1, Y1} | L]);
        false -> %% 行不通
            case triangle_diag({X1, Y1}, {X3, Y3}, Walkable, []) of
                [] -> %% 直达
                    do_smooth_triangle([{X1, Y1}, {X3, Y3} | T], Walkable, L);
                [I = {_, _}] ->
                    do_smooth_triangle([I, {X3, Y3} | T], Walkable, [{X1, Y1} | L]);
                false -> %% 行不通
                    do_smooth_triangle([{X2, Y2}, {X3, Y3} | T], Walkable, [{X1, Y1} | L])
            end
    end;
do_smooth_triangle([{X1, Y1}, {X2, Y2}, {X3, Y3} | T], Walkable, L) -> %% 最后为直线方法，则本次优化直线
    case triangle_diag({X1, Y1}, {X3, Y3}, Walkable, []) of 
        [] -> %% 直达
            do_smooth_triangle([{X1, Y1}, {X3, Y3} | T], Walkable, L);
        [I = {_, _}] -> %% 有转角
            do_smooth_triangle([I, {X3, Y3} | T], Walkable, [{X1, Y1} | L]);
        false -> %% 行不通
            %% do_smooth_triangle([{X2, Y2}, {X3, Y3} | T], Walkable, [{X1, Y1} | L])
            case triangle_straight({X1, Y1}, {X3, Y3}, Walkable, []) of
                [] -> %% 直达
                    do_smooth_triangle([{X1, Y1}, {X3, Y3} | T], Walkable, L);
                [I = {_, _}] ->
                    do_smooth_triangle([I, {X3, Y3} | T], Walkable, [{X1, Y1} | L]);
                false -> %% 行不通
                    do_smooth_triangle([{X2, Y2}, {X3, Y3} | T], Walkable, [{X1, Y1} | L])
            end
    end;
do_smooth_triangle([P | T], Walkable, L) ->
    do_smooth_triangle(T, Walkable, [P | L]).

%% @doc 等边三角形 先直线再斜线方式
triangle_straight(Tp, Tp, _Walkable, L) -> %% 目标点
    L;
triangle_straight({Sx, Sy}, {Tx, Ty}, Walkable, L) when abs(Sx - Tx) =:= abs(Sy - Ty) -> %% 形成等边三角 走斜边 45/135/225/315度
    NextP = {?next_p(Sx, Tx), ?next_p(Sy, Ty)},
    case dict:find(NextP, Walkable) of
        {ok, 1} -> triangle_straight(NextP, {Tx, Ty}, Walkable, L);
        _ -> false %% 此路走不通 放弃
    end;
triangle_straight({Sx, Sy}, {Tx, Ty}, Walkable, _) when abs(Sx - Tx) > abs(Sy - Ty) -> %% 先走直线X段
    NextP = {?next_p(Sx, Tx), Sy},
    case dict:find(NextP, Walkable) of
        {ok, 1} -> triangle_straight(NextP, {Tx, Ty}, Walkable, [NextP]);
        _ -> false %% 此路走不通 放弃
    end;
triangle_straight({Sx, Sy}, {Tx, Ty}, Walkable, _) -> %% 先走直线Y段
    NextP = {Sx, ?next_p(Sy, Ty)},
    case dict:find(NextP, Walkable) of
        {ok, 1} -> triangle_straight(NextP, {Tx, Ty}, Walkable, [NextP]);
        _ -> false %% 此路走不通 放弃
    end.

%% @doc 等边三角形 先斜线再直线方式
triangle_diag(Tp, Tp, _Walkable, L) -> %% 目标点
    L;
triangle_diag({Sx, Sy}, {Tx, Ty}, Walkable, L) ->
    NextP = {?next_p(Sx, Tx), ?next_p(Sy, Ty)},
    case dict:find(NextP, Walkable) of
        {ok, 1} when Sx =/= Tx andalso Sy =/= Ty -> triangle_diag(NextP, {Tx, Ty}, Walkable, [NextP]);
        {ok, 1} -> triangle_diag(NextP, {Tx, Ty}, Walkable, L);
        _ -> false %% 此路走不通 放弃
    end.

%% doc A*算法寻路 空列表表示失败
-spec find({Sx, Sy}, {Tx, Ty}, Walkable, Args) -> [[#a_path{}]] when 
    Sx  :: non_neg_integer(),  %% 开始X坐标.
    Sy  :: non_neg_integer(),   %% 开始Y坐标
    Tx  :: non_neg_integer(),   %% 目标X坐标
    Ty  :: non_neg_integer(),   %% 目标Y坐标 
    Walkable    :: [{non_neg_integer(), non_neg_integer()}], %% 可行走区域
    Args        :: [A],
    A           :: 
         {num, pos_integer()} %% 查找路径数量
        | {cost, pos_integer()} %% 上下左右移动代价
        | {lf_cost, pos_integer()} %% 左右移动代价
        | {ud_cost, pos_integer()} %% 上下移动代价
        | {diag_cost, pos_integer()} %% 对角移动代价
        | {diag, false | true} %% 如果相邻有不可走区域 是否走斜角
        | {tran, [tuple()]}. %% 连通点 中转站 跳点 {{ToX, ToY}, {X, Y}, F, Data}
find({X, Y}, {X, Y}, _Walkable, _Args) ->
    [];
find({Sx, Sy}, {Tx, Ty}, Walkable, Args) ->
    Num = find_args(num, Args, 1),
    Cost = find_args(lf_cost, Args, 10),
    LFcost = find_args(lf_cost, Args, Cost),
    UDcost = find_args(ud_cost, Args, Cost),
    DiagCost = find_args(diag_cost, Args, round(math:sqrt(LFcost * LFcost + UDcost * UDcost))),
    Diag = find_args(diag, Args, false),
    TranL = find_args(tran, Args, []),
    NewTranL = [#a_path{node = P, parent_node = ParentP, f = F, data = Data} || {P, ParentP, F, Data} <- TranL],
    APath = #a_path_tmp{
        sx = Sx, sy = Sy, tx = Tx, ty = Ty, diag = Diag, tran = NewTranL
        ,lf_cost = LFcost, ud_cost = UDcost, diag_cost = DiagCost
    },
    D = dict:from_list([{P, 1} || P <- Walkable]),
    Start = #a_path{node = {Sx, Sy}},
    do_find(Start, APath, D, Num, []).

%% @doc 广度优化查找 
find_vast({X, Y}, {X, Y}, _Walkable, _Args) ->
    [];
find_vast({Sx, Sy}, {Tx, Ty}, Walkable, Args) ->
    Num = find_args(num, Args, 1),
    TranL = find_args(tran, Args, []),
    NewTranL = [#a_path{node = P, parent_node = ParentP, f = F, data = Data} || {P, ParentP, F, Data} <- TranL],
    Diag = find_args(diag, Args, false),
    APath = #a_path_tmp{
        sx = Sx, sy = Sy, tx = Tx, ty = Ty, diag = Diag, tran = NewTranL
    },
    D = dict:from_list([{P, 1} || P <- Walkable, P =/= {Sx, Sy}]),
    Start = #a_path{node = {Sx, Sy}},
    do_find_vast([Start], [], 1, APath, D, Num, []).

%%-------------------------------
%% 内部方法
%%-------------------------------

%% 查找参数
find_args(Key, Args, DefVal) ->
    case lists:keyfind(Key, 1, Args) of
        {Key, Val} -> Val;
        _ -> DefVal
    end.

%%----------------------------------------------------
%% 以下为A*寻路方法
%%----------------------------------------------------

%% 寻路查找 直到出结果或是开启列表为空
do_find(undefined, #a_path_tmp{sx = _Sx, sy = _Sy, tx = _Tx, ty = _Ty}, _Walkable, _Num, L) -> %% 寻路失败
    ?print("find fail: ~w =/= ~w sx = ~w, sy = ~w, tx = ~w, ty = ~w~n", [_Num, length(L), _Sx, _Sy, _Tx, _Ty]),
    L;
do_find(Tree, APath = #a_path_tmp{tran = TranL}, Walkable, Num, L) ->
    {MinNode = #a_path{node = P, f = F}, OpenTree} = find_min_node(Tree),
    Open = [Node1#a_path{f = F + F1} || Node1 = #a_path{node = P1, parent_node = ParentNode, f = F1} <- TranL, ParentNode =:= P, dict:find(P1, Walkable) =:= {ok, 1}],
    {NewTree, NewW} = add_to_open(Open, OpenTree, Walkable),
    NewWalk = dict:store(P, MinNode#a_path{status = close}, NewW),
    TestNodes = get_test_node(MinNode, APath),
    open_test_node(TestNodes, MinNode, false, NewTree, APath, NewWalk, Num, L).
find_min_node(Node = #a_path{l_tree = undefined, r_tree = RTree}) -> %% 当前节点是最小的
    {Node#a_path{r_tree = undefined, r_num = 0}, RTree};
find_min_node(Node = #a_path{l_tree = LTree, l_num = LNum}) ->
    {MinNode, NewLTree} = find_min_node(LTree),
    {MinNode, Node#a_path{l_tree = NewLTree, l_num = LNum - 1}}.
%% find_max_node(Node = #a_path{r_tree = undefined, l_tree = LTree}) -> %% 当前节点是最大的
%%     {Node#a_path{l_tree = undefined, l_num = 0}, LTree};
%% find_max_node(Node = #a_path{r_tree = RTree, r_num = RNum}) ->
%%     {MaxNode, NewRTree} = find_max_node(RTree),
%%     {MaxNode, Node#a_path{r_tree = NewRTree, r_num = RNum - 1}}.

%% @doc 增加节点到开启列表
add_to_open([Node = #a_path{node = P} | T], Tree, Walkable) ->
    NewTree = add_to_open_tree(Node, Tree),
    NewWalk = dict:store(P, Node, Walkable),
    add_to_open(T, NewTree, NewWalk);
add_to_open([], Tree, Walkable) ->
    {Tree, Walkable}.

%% 向开启树插入数据
add_to_open_tree(Node, undefined) ->
    Node#a_path{l_tree = undefined, l_num = 0, r_tree = undefined, r_num = 0};
%% add_to_open_tree(Node = #a_path{node = P, f = F1}, Tree = #a_path{f = F, l_num = LNum, r_num = RNum}) when F1 < F andalso LNum > RNum andalso ((LNum + RNum) div 3 + 1) < (LNum - RNum) -> 
%%     Tree1 = #a_path{f = F2} = del_open_tree(Tree, Tree),
%%     Tree2 = #a_path{f = F3} = add_to_open_tree(Tree, Tree1),
%%     add_to_open_tree(Node, Tree2),
add_to_open_tree(Node = #a_path{f = F1}, Tree = #a_path{f = F, l_tree = LTree, l_num = LNum}) when F1 < F -> 
    Tree#a_path{l_tree = add_to_open_tree(Node, LTree), l_num = LNum + 1};
%% 存在值相等问题 不能处理 会出现死循环
%% add_to_open_tree(Node, Tree = #a_path{l_num = LNum, r_num = RNum}) when RNum > LNum andalso ((RNum + LNum) div 3 + 1) < (RNum - LNum) -> 
%%     Tree1 = del_open_tree(Tree, Tree),
%%     Tree2 = add_to_open_tree(Node, Tree1),
%%     add_to_open_tree(Tree, Tree2);
add_to_open_tree(Node, Tree = #a_path{r_tree = RTree, r_num = RNum}) ->
    Tree#a_path{r_tree = add_to_open_tree(Node, RTree), r_num = RNum + 1}.

%% %% 判断树是否有序
%% check_sort(undefined) -> true;
%% check_sort(#a_path{l_tree = undefined, r_tree = undefined}) -> true;
%% check_sort(#a_path{f = F, l_tree = LTree = #a_path{f = LF}, r_tree = undefined}) -> F > LF andalso check_sort(LTree);
%% check_sort(#a_path{f = F, l_tree = undefined, r_tree = RTree = #a_path{f = RF}}) -> F =< RF andalso check_sort(RTree);
%% check_sort(#a_path{f = F, l_tree = LTree = #a_path{f = LF}, r_tree = RTree = #a_path{f = RF}}) ->
%%     F > LF andalso F =< RF andalso check_sort(LTree) andalso check_sort(RTree).

%% %% 打印输出树
%% print_tree(undefined) -> ok;
%% print_tree(#a_path{l_tree = undefined, r_tree = undefined}) -> ok;
%% print_tree(#a_path{f = F, l_tree = LTree = #a_path{f = LF}, r_tree = undefined}) -> 
%%     ?print("[~w]<====left===[~w]~n", [LF, F]),
%%     print_tree(LTree);
%% print_tree(#a_path{f = F, l_tree = undefined, r_tree = RTree = #a_path{f = RF}}) ->
%%     ?print("[~w]====right===>[~w]~n", [F, RF]),
%%     print_tree(RTree);
%% print_tree(#a_path{f = F, l_tree = LTree = #a_path{f = LF}, r_tree = RTree = #a_path{f = RF}}) ->
%%     ?print("[~w]<====left===[~w]====right====>[~w]~n", [LF, F, RF]),
%%     print_tree(LTree),
%%     print_tree(RTree).

%% 从树中删除节点
del_open_tree(#a_path{node = P}, #a_path{node = P, l_tree = undefined, r_tree = RTree}) ->
    RTree;
del_open_tree(#a_path{node = P}, #a_path{node = P, l_tree = LTree, r_tree = undefined}) ->
    LTree;
%% del_open_tree(#a_path{node = P}, #a_path{node = P, l_tree = LTree, l_num = LNum, r_tree = RTree, r_num = RNum}) when LNum > RNum ->
%%     {Node, NewLTree} = find_max_node(LTree),
%%     Node#a_path{r_tree = RTree, r_num = RNum, l_tree = NewLTree, l_num = LNum - 1};
del_open_tree(#a_path{node = P}, #a_path{node = P, l_tree = LTree, l_num = LNum, r_tree = RTree, r_num = RNum}) ->
    {Node, NewRTree} = find_min_node(RTree),
    Node#a_path{l_tree = LTree, l_num = LNum, r_tree = NewRTree, r_num = RNum - 1};
del_open_tree(Node = #a_path{f = F1}, Tree = #a_path{f = F, l_tree = LTree, l_num = LNum}) when F1 < F ->
    Tree#a_path{l_tree = del_open_tree(Node, LTree), l_num = LNum - 1};
del_open_tree(Node, Tree = #a_path{r_tree = RTree, r_num = RNum}) ->
    Tree#a_path{r_tree = del_open_tree(Node, RTree), r_num = RNum - 1};
del_open_tree(_Node = #a_path{node = _P, f = _F}, Tree) ->
    ?print("error del tree node[~w] ~w fail: not_find ~w~n", [_P, _F, Tree]),
    Tree.

%% 更新树节点
update_open_tree(OldNode, Node, Tree) ->
    NewTree = del_open_tree(OldNode, Tree),
    add_to_open_tree(Node, NewTree).

%% 测试节点放到开启列表
open_test_node([], _MinNode, _Flag, Open, APath, Walkable, Num, L) ->
    do_find(Open, APath, Walkable, Num, L);
open_test_node([I = {{X, Y}, _Cost, _Type} | _T], MinNode, _Flag, Open, APath = #a_path_tmp{tx = X, ty = Y}, Walkable, Num, L) -> %% 目标格子
    Node = to_node(I, MinNode, APath),
    NewL = [to_list_find(Node, Walkable, []) | L],
    case length(NewL) < Num of
        true -> do_find(Open, APath, Walkable, Num, NewL);
        _ -> NewL 
    end;
open_test_node([{_P, _Cost, Type} | T], MinNode, true, Open, APath = #a_path_tmp{diag = false}, Walkable, Num, L) when Type > 4 ->
    open_test_node(T, MinNode, true, Open, APath, Walkable, Num, L);
open_test_node([I = {P, _Cost, _Type} | T], MinNode, Flag, Open, APath, Walkable, Num, L) ->
    Node = #a_path{g = G} = to_node(I, MinNode, APath),
    case dict:find(P, Walkable) of
        error -> %% 不可行走 丢弃
            open_test_node(T, MinNode, true, Open, APath, Walkable, Num, L);
        {ok, 1} -> %% 可行走
            NewOpen = add_to_open_tree(Node, Open),
            NewWalk = dict:store(P, Node, Walkable),
            open_test_node(T, MinNode, Flag, NewOpen, APath, NewWalk, Num, L);
        {ok, OldNode = #a_path{g = OldG, status = open}} when G < OldG ->
            NewOpen = update_open_tree(OldNode, Node, Open),
            NewWalk = dict:store(P, Node, Walkable),
            open_test_node(T, MinNode, Flag, NewOpen, APath, NewWalk, Num, L);
        {ok, _} -> %% 已在关闭列表中 丢弃
            open_test_node(T, MinNode, Flag, Open, APath, Walkable, Num, L)
    end.

%% 转成格子数据
to_node({{X, Y}, Cost, Type}, #a_path{node = P, g = ParentG}, #a_path_tmp{sx = Sx, sy = Sy, tx = Tx, ty = Ty, lf_cost = LFcost, ud_cost = UDcost, diag_cost = _DiagCost}) ->
    G = ParentG + Cost, 
    %% S1 = abs(Tx - X), 
    %% S2 = abs(Ty - Y),
    %% H = case S1 > S2 of
    %%     true -> 1.414 * S2 * DiagCost + (S1 - S2) * LFcost;
    %%     false -> 1.414 * S1 * DiagCost + (S2 - S1) * UDcost
    %% end,
    S1 = abs(Tx - X), 
    S2 = abs(Ty - Y),
    S3 = abs(Sx - X),
    S4 = abs(Sy - Y),
    H = (S1 * LFcost + S2 * UDcost) * (S3 + S4) div (S1 + S2 + S3 + S4),
    #a_path{node = {X, Y}, parent_node = P, type = Type, g = G, h = H, f = G + H}. 

%% 列出结果
to_list_find(Node = #a_path{parent_node = P}, Close, L) ->
    case dict:find(P, Close) of
        {ok, ParentNode = #a_path{}} ->
            to_list_find(ParentNode, Close, [Node | L]);
        _ ->
            [Node | L]
    end.

%%------------------------------------------
%% 广度优化 一层层处理 
%%------------------------------------------

do_find_vast([], [], _N, #a_path_tmp{sx = _Sx, sy = _Sy, tx = _Tx, ty = _Ty}, _Walkable, _Num, L) -> %% 查找失败
    ?print("find_vast fail: ~w =/= ~w sx = ~w, sy = ~w, tx = ~w, ty = ~w~n", [_Num, length(L), _Sx, _Sy, _Tx, _Ty]),
    L;
do_find_vast([], ChildL, N, APath, Walkable, Num, L) ->
    do_find_vast(lists:reverse(ChildL), [], N + 1, APath, Walkable, Num, L);
do_find_vast([Node = #a_path{node = P} | T2], ChildL, N, APath = #a_path_tmp{tran = TranL}, Walkable, Num, L) ->
    TestNodes = get_test_node(Node, APath),
    OpenL = [Node1 || Node1 = #a_path{node = P1, parent_node = ParentNode} <- TranL, ParentNode =:= P, dict:find(P1, Walkable) =:= {ok, 1}],
    NewWalk = add_to_walkable(OpenL, Walkable),
    test_child_node_vast(TestNodes, false, Node, T2, OpenL ++ ChildL, N, APath, NewWalk, Num, L).
test_child_node_vast([], _Flag, _ParentNode, T2, ChildL, N, APath, Walkable, Num, L) ->
    do_find_vast(T2, ChildL, N, APath, Walkable, Num, L);
test_child_node_vast([{{X, Y}, _Cost, _Type} | _T], _Flag, #a_path{node = ParentP}, T2, ChildL, N, APath = #a_path_tmp{tx = X, ty = Y}, Walkable, Num, L) ->
    Node = #a_path{node = {X, Y}, parent_node = ParentP},
    NewL = [to_list_find(Node, Walkable, []) | L],
    case length(NewL) < Num of
        true -> do_find_vast(T2, ChildL, N, APath, Walkable, Num, NewL);
        _ -> NewL
    end;
test_child_node_vast([{_P, _Cost, Type} | T], true, ParentNode, T2, ChildL, N, APath = #a_path_tmp{diag = false}, Walkable, Num, L) when Type > 4 ->
    test_child_node_vast(T, true, ParentNode, T2, ChildL, N, APath, Walkable, Num, L);
test_child_node_vast([{P, _Cost, Type} | T], Flag, ParentNode = #a_path{node = ParentP}, T2, ChildL, N, APath, Walkable, Num, L) ->
    case dict:find(P, Walkable) of
        {ok, 1} -> %% 可行走区域
            Node = #a_path{node = P, parent_node = ParentP, type = Type},
            NewWalk = dict:store(P, Node, Walkable), %% 已有线路通过 设置为不可行走
            test_child_node_vast(T, Flag, ParentNode, T2, [Node | ChildL], N, APath, NewWalk, Num, L);
        {ok, _} -> %% 已关闭区域 
            test_child_node_vast(T, Flag, ParentNode, T2, ChildL, N, APath, Walkable, Num, L);
        _ -> %% 不可行走区域
            test_child_node_vast(T, true, ParentNode, T2, ChildL, N, APath, Walkable, Num, L)
    end.
add_to_walkable([], Walkable) ->
    Walkable;
add_to_walkable([Node = #a_path{node = P} | T], Walkable) ->
    NewWalk = dict:store(P, Node, Walkable),
    add_to_walkable(T, NewWalk).

%% 获取下一步检查节点
get_test_node(#a_path{node = {X, Y}, type = 1}, #a_path_tmp{lf_cost = LFcost, ud_cost = UDcost, diag_cost = DiagCost}) -> %% 向左
    [
        {?a_path_type_1(X, Y), LFcost, 1}
        ,{?a_path_type_3(X, Y), UDcost, 3}
        ,{?a_path_type_4(X, Y), UDcost, 4}
        ,{?a_path_type_5(X, Y), DiagCost, 5}
        ,{?a_path_type_6(X, Y), DiagCost, 6}
    ];
get_test_node(#a_path{node = {X, Y}, type = 2}, #a_path_tmp{lf_cost = LFcost, ud_cost = UDcost, diag_cost = DiagCost}) -> %% 向右
    [
        {?a_path_type_2(X, Y), LFcost, 2}
        ,{?a_path_type_3(X, Y), UDcost, 3}
        ,{?a_path_type_4(X, Y), UDcost, 4}
        ,{?a_path_type_7(X, Y), DiagCost, 7}
        ,{?a_path_type_8(X, Y), DiagCost, 8}
    ];
get_test_node(#a_path{node = {X, Y}, type = 3}, #a_path_tmp{lf_cost = LFcost, ud_cost = UDcost, diag_cost = DiagCost}) -> %% 向上
    [
        {?a_path_type_1(X, Y), LFcost, 1}
        ,{?a_path_type_2(X, Y), LFcost, 2}
        ,{?a_path_type_3(X, Y), UDcost, 3}
        ,{?a_path_type_5(X, Y), DiagCost, 5}
        ,{?a_path_type_8(X, Y), DiagCost, 8}
    ];
get_test_node(#a_path{node = {X, Y}, type = 4}, #a_path_tmp{lf_cost = LFcost, ud_cost = UDcost, diag_cost = DiagCost}) -> %% 向下
    [
        {?a_path_type_1(X, Y), LFcost, 1}
        ,{?a_path_type_2(X, Y), LFcost, 2}
        ,{?a_path_type_4(X, Y), UDcost, 4}
        ,{?a_path_type_6(X, Y), DiagCost, 6}
        ,{?a_path_type_7(X, Y), DiagCost, 7}
    ];
get_test_node(#a_path{node = {X, Y}, type = 5}, #a_path_tmp{lf_cost = LFcost, ud_cost = UDcost, diag_cost = DiagCost}) -> %% 左上
    [
        {?a_path_type_1(X, Y), LFcost, 1}
        ,{?a_path_type_3(X, Y), UDcost, 3}
        ,{?a_path_type_5(X, Y), DiagCost, 5}
        ,{?a_path_type_6(X, Y), DiagCost, 6}
        ,{?a_path_type_8(X, Y), DiagCost, 8}
    ];
get_test_node(#a_path{node = {X, Y}, type = 6}, #a_path_tmp{lf_cost = LFcost, ud_cost = UDcost, diag_cost = DiagCost}) -> %% 左下
    [
        {?a_path_type_2(X, Y), LFcost, 2}
        ,{?a_path_type_4(X, Y), UDcost, 4}
        ,{?a_path_type_5(X, Y), DiagCost, 5}
        ,{?a_path_type_6(X, Y), DiagCost, 6}
        ,{?a_path_type_7(X, Y), DiagCost, 7}
    ];
get_test_node(#a_path{node = {X, Y}, type = 7}, #a_path_tmp{lf_cost = LFcost, ud_cost = UDcost, diag_cost = DiagCost}) -> %% 右下
    [
        {?a_path_type_2(X, Y), LFcost, 2}
        ,{?a_path_type_4(X, Y), UDcost, 4}
        ,{?a_path_type_6(X, Y), DiagCost, 6}
        ,{?a_path_type_7(X, Y), DiagCost, 7}
        ,{?a_path_type_8(X, Y), DiagCost, 8}
    ];
get_test_node(#a_path{node = {X, Y}, type = 8}, #a_path_tmp{lf_cost = LFcost, ud_cost = UDcost, diag_cost = DiagCost}) -> %% 右上
    [
        {?a_path_type_2(X, Y), LFcost, 2}
        ,{?a_path_type_3(X, Y), UDcost, 3}
        ,{?a_path_type_5(X, Y), DiagCost, 5}
        ,{?a_path_type_7(X, Y), DiagCost, 7}
        ,{?a_path_type_8(X, Y), DiagCost, 8}
    ];
get_test_node(#a_path{node = {X, Y}}, #a_path_tmp{lf_cost = LFcost, ud_cost = UDcost, diag_cost = DiagCost}) ->
    [
        {?a_path_type_1(X, Y), LFcost, 1}
        ,{?a_path_type_2(X, Y), LFcost, 2}
        ,{?a_path_type_3(X, Y), UDcost, 3}
        ,{?a_path_type_4(X, Y), UDcost, 4}
        ,{?a_path_type_5(X, Y), DiagCost, 5}
        ,{?a_path_type_6(X, Y), DiagCost, 6}
        ,{?a_path_type_7(X, Y), DiagCost, 7}
        ,{?a_path_type_8(X, Y), DiagCost, 8}
    ].
